#include "pid.h"

pid motor_pid;

void motor_contral(void)
{
	motor_pid.kp=1.0f;
	motor_pid.ki=0.3f;
	motor_pid.kd=0.5f;
	
	
	motor_pid.Actual = motor_pulse_num;
	motor_pid.Error2=motor_pid.Error1;
	motor_pid.Error1=motor_pid.Error0;
	motor_pid.Error0=motor_pid.Target-motor_pid.Actual;
	motor_pid.Out += motor_pid.kp *  (motor_pid.Error0-motor_pid.Error1)
					+ motor_pid.ki* motor_pid.Error0
					+ motor_pid.kd * (motor_pid.Error0-2*motor_pid.Error1+motor_pid.Error2);
	
//	if(motor_pid.Out > 0)
//	{
//		if(motor_pid.Out > 100)motor_pid.Out = 100;
//		
////		HAL_GPIO_WritePin(motor_dir_GPIO_Port,motor_dir_Pin,GPIO_PIN_SET);
//		
//		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,100-motor_pid.Out);
//	}
	
	
	
//	if(motor_pid.Out < 0)
//	{
//		if(motor_pid.Out < -100)motor_pid.Out = -100;
//		motor_pid.Out = -motor_pid.Out;
//		HAL_GPIO_WritePin(motor_dir_GPIO_Port,motor_dir_Pin,GPIO_PIN_RESET);
//		__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,100-motor_pid.Out);
//	}

	if(motor_pid.Out < 0)
	{
		motor_pid.Out=-motor_pid.Out;
	}
	if(motor_pid.Out > 100)motor_pid.Out = 100;
	__HAL_TIM_SetCompare(&htim1,TIM_CHANNEL_4,100-motor_pid.Out);
	

}





